#include "utility.h"
// #include "mrandom.hpp"
#include <Eigen/Dense>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>

using namespace std;
using namespace Eigen;

void ReadMeshASSCII(const std::string &meshFname, VarArray<Point, 2, 0> &grid) {
  cout << "start reading mesh file\n";

  ifstream meshF{meshFname};
  if (!meshF) {
    cerr << "open mesh file err\n" << endl;
    exit(EXIT_FAILURE);
  }

  int ni, nj;
  meshF >> ni >> nj;

  // 跟 blazik 同步 后面有需求再改
  grid.ReConstruct(ni, nj);

  for (int j = 0; j < nj; ++j)
    for (int i = 0; i < ni; ++i) {
      meshF >> grid(i, j).x;
    }

  for (int j = 0; j < nj; ++j)
    for (int i = 0; i < ni; ++i) {
      meshF >> grid(i, j).y;
    }

  meshF.close();
  cout << "read mesh file success" << endl;
}

void InitializeFlow(VarArray<Variable, 2, 0> &flow) {

  Variable zero{0.0, 0.0};
  flow = zero;
  const int np_in_ydir = flow.Dim(1);
  flow(0, 0).u = 0.0;
  flow(0, 0).v = 0.0;
  // 给第一列赋初值
  for (int j = 1; j < np_in_ydir; ++j) {
    flow(0, j).u = 1.0;
    flow(0, j).v = 0.0;
  }
  const int np_in_xdir{flow.Dim(0)};
  // 给上下边界赋值
  for (int i = 1; i < np_in_xdir; ++i) {
    flow(i, 0).u = 0.0;
    flow(i, 0).v = 0.0;

    // flow(i, np_in_ydir - 1).u = 1.0;
    // flow(i, np_in_ydir - 1).v = 0.0;
  }
}

// 计算之前未知时间层需要使用已知时间层初始化
void CalcJacobianMatrix(const VarArray<Variable, 2, 0> &flow, int column,
                        const VarArray<Point, 2, 0> &grid,
                        VarArray<double, 2, 0> &mat_coef, const double re_num) {
  int npoint_in_ydir{grid.Dim(1)};
  const double C{1 / re_num};

  // 先统一处理 最后考虑首尾
  for (int j = 1; j < npoint_in_ydir - 1; ++j) {

    const double rJacob = 2.0 / (grid(column, j + 1).y - grid(column, j - 1).y);
    const double B =
        grid(column, j + 1).y - 2 * grid(column, j).y + grid(column, j - 1).y;
    const double rDeltx = 1.0 / (grid(column, j).x - grid(column - 1, j).x);
    // 第一次迭代第i层时间步上的值由上一层近视
    // if(iter_num != 1)

    mat_coef(0, j) = -0.5 * flow(column, j).v * rJacob - C * rJacob * rJacob -
                     0.5 * B * rJacob * rJacob * C * rJacob;

    mat_coef(1, j) =
        (2.0 * flow(column, j).u - flow(column - 1, j).u) * rDeltx +
        2.0 * C * rJacob * rJacob;
    mat_coef(2, j) = 0.5 * flow(column, j).v * rJacob - C * rJacob * rJacob +
                     0.5 * B * rJacob * rJacob * C * rJacob;
  }
  //   cout<< flow(column, npoint_in_ydir-1).u << endl;
  // 首尾处理
  mat_coef(0, 1) = 0.0;

  const double rJacob = 2.0 / (grid(column, npoint_in_ydir - 2 + 1).y -
                               grid(column, npoint_in_ydir - 2 - 1).y);
  const double B = grid(column, npoint_in_ydir - 2 + 1).y -
                   2 * grid(column, npoint_in_ydir - 2).y +
                   grid(column, npoint_in_ydir - 2 - 1).y;
  const double rDeltx = 1.0 / (grid(column, npoint_in_ydir - 2).x -
                               grid(column - 1, npoint_in_ydir - 2).x);
  mat_coef(1, npoint_in_ydir - 2) +=
      0.5 * flow(column, npoint_in_ydir - 2).v * rJacob - C * rJacob * rJacob +
      0.5 * B * rJacob * rJacob * C * rJacob;
  mat_coef(2, npoint_in_ydir - 2) = 0.0;
}

void InitializeNextLine(VarArray<Variable, 2, 0> &flow, int column) {
  int npoint_in_ydir{flow.Dim(1)};
  for (int j = 1; j < npoint_in_ydir; ++j) {
    flow(column, j) = flow(column - 1, j);
  }
}

void CalcFXk(const VarArray<Variable, 2, 0> &flow, int column,
             const VarArray<Point, 2, 0> &grid, const double re_num,
             vector<double> &fx_k)

{
  int npoint_in_ydir{grid.Dim(1)};
  const double C{1 / re_num};

  for (int j = 1; j < npoint_in_ydir - 1; ++j) {
    const double rJacob = 2.0 / (grid(column, j + 1).y - grid(column, j - 1).y);
    const double B =
        grid(column, j + 1).y - 2 * grid(column, j).y + grid(column, j - 1).y;
    const double rDeltx = 1.0 / (grid(column, j).x - grid(column - 1, j).x);

    const Variable &var_i_j{flow(column, j)}, var_im1_j{flow(column - 1, j)},
        var_i_jm1{flow(column, j - 1)}, var_i_jp1{flow(column, j + 1)};

    fx_k[j] = var_i_j.u * (var_i_j.u - var_im1_j.u) * rDeltx +
              0.5 * var_i_j.v * (var_i_jp1.u - var_i_jm1.u) * rJacob -
              C * rJacob * rJacob *
                  (var_i_jp1.u - 2 * var_i_j.u + var_i_jm1.u -
                   0.5 * (var_i_jp1.u - var_i_jm1.u) * rJacob * B);
  }
}

double NewtonIterForU(VarArray<Variable, 2, 0> &flow, const int column,
                      const VarArray<Point, 2, 0> &grid, const double re_num) {

  int count = 1, npoint_in_ydir{flow.Dim(1)};

  VarArray<double, 2, 0> mat_coef(3, npoint_in_ydir);
  vector<double> fx_k(npoint_in_ydir);

  vector<double> old_u(npoint_in_ydir);
  for (int j = 1; j < npoint_in_ydir - 1; ++j)
    old_u[j] = flow(column, j).u;

  double res1, resn, res_log;
  do {

    CalcJacobianMatrix(flow, column, grid, mat_coef, re_num);
    CalcFXk(flow, column, grid, re_num, fx_k);
    ThomasAlgorithm(mat_coef, fx_k.data(), 1, fx_k.size() - 1);

    UpdateU(fx_k, flow, column);

    resn = CalcNormal(fx_k);
    if (count == 1)
      res1 = resn + 1.0e-30;
    res_log = log10(resn / res1);
    ++count;
  } while (res_log > -13.0 && resn > 1.0e-13 && count < 80);

  //   cout << "count = " << count << endl;

  double res_u{0.0};
  for (int j = 1; j < npoint_in_ydir - 1; ++j)
    res_u += (flow(column, j).u - old_u[j]) * (flow(column, j).u - old_u[j]);
  //   UpdateU(fx_k, flow, column);

  //   cout << "u = " << endl;
  //   for (int j = 0; j < npoint_in_ydir; ++j)
  //     cout << flow(column, j).u << endl;

  //   exit(EXIT_FAILURE);

  return sqrt(res_u);
}

void UpdateU(const vector<double> &deltaU, VarArray<Variable, 2, 0> &flow,
             int column) {
  for (int j = 1; j < deltaU.size() - 1; ++j) {
    flow(column, j).u -= deltaU[j];
  }
}

double CalcNormal(const vector<double> &deltU) {
  double tmp{0.0};
  for (int j = 1; j < deltU.size() - 1; ++j)
    tmp += deltU[j] * deltU[j];
  return sqrt(tmp);
}

double UpdateVfirstOrder(VarArray<Variable, 2, 0> &flow, const int column,
                         const VarArray<Point, 2, 0> &grid) {

  int npoint_in_ydir{flow.Dim(1)};

  vector<double> old_v(npoint_in_ydir);
  for (int j = 1; j < npoint_in_ydir - 1; ++j)
    old_v[j] = flow(column, j).v;

  for (int j = 0; j < npoint_in_ydir; ++j)
    cout << flow(column, j).v << endl;
  cout << "end v" << endl;

  for (int j = 1; j < npoint_in_ydir - 1; ++j) {
    const double Jacob = 0.5 * (grid(column, j + 1).y - grid(column, j - 1).y);
    const double deltx = (grid(column, j).x - grid(column - 1, j).x);
    cout << "Jacob = " << Jacob << endl;
    cout << "deltx = " << deltx << endl;
    flow(column, j).v =
        flow(column, j - 1).v -
        Jacob / deltx * (flow(column, j).u - flow(column - 1, j).u);
  }

  for (int j = 0; j < npoint_in_ydir; ++j)
    cout << flow(column, j).v << endl;
  cout << "end v" << endl;
  exit(EXIT_FAILURE);

  double res_v{0.0};
  for (int j = 1; j < npoint_in_ydir - 1; ++j)
    res_v += (flow(column, j).v - old_v[j]) * (flow(column, j).v - old_v[j]);
  return sqrt(res_v);
}

double UpdateVsecondOrder(VarArray<Variable, 2, 0> &flow, const int column,
                          const VarArray<Point, 2, 0> &grid) {

  int npoint_in_ydir{flow.Dim(1)};

  vector<double> old_v(npoint_in_ydir);
  for (int j = 1; j < npoint_in_ydir - 1; ++j)
    old_v[j] = flow(column, j).v;

  //   for (int j = 0; j < npoint_in_ydir; ++j)
  //     cout << flow(column, j).v << endl;
  //   cout << "end v" << endl;

  int size = npoint_in_ydir - 2;
  MatrixXd coef = MatrixXd::Zero(size, size);
  coef(0, 1) = 1;
  for (int j = 1; j < size - 1; ++j) {
    coef(j, j + 1) = 1;
    coef(j, j - 1) = -1;
  }
  coef(size - 1, size - 2) = -1;

  VectorXd fxb(size);
  for (int j = 0; j < size; ++j) {
    int jp1 = j + 1;
    const double Jacob =
        0.5 * (grid(column, jp1 + 1).y - grid(column, jp1 - 1).y);
    const double deltx = (grid(column, jp1).x - grid(column - 1, jp1).x);
    fxb(j) =
        2.0 * Jacob / deltx * (flow(column, jp1).u - flow(column - 1, jp1).u);
  }
  fxb *= -1.0;

  VectorXd new_v = coef.colPivHouseholderQr().solve(fxb);

  // update
  for (int j = 0; j < size; ++j)
    flow(column, j + 1).v = new_v(j);

  for (int j = 0; j < npoint_in_ydir; ++j)
    cout << flow(column, j).v << endl;
  cout << "end v" << endl;
  exit(EXIT_FAILURE);

  double res_v{0.0};
  for (int j = 1; j < npoint_in_ydir - 1; ++j)
    res_v += (flow(column, j).v - old_v[j]) * (flow(column, j).v - old_v[j]);
  return sqrt(res_v);
}

double UpdateVthirdOrder(VarArray<Variable, 2, 0> &flow, const int column,
                         const VarArray<Point, 2, 0> &grid) {

  int npoint_in_ydir{flow.Dim(1)};

  vector<double> old_v(npoint_in_ydir);
  for (int j = 1; j < npoint_in_ydir - 1; ++j)
    old_v[j] = flow(column, j).v;

  //   for (int j = 0; j < npoint_in_ydir; ++j)
  //     cout << flow(column, j).v << endl;
  //   cout << "end v" << endl;

  int size = npoint_in_ydir - 2;
  MatrixXd coef = MatrixXd::Zero(size, size);
  coef(0, 0) = -3;
  coef(0, 1) = 6;
  coef(0, 2) = -1;

  for (int j = 1; j < size - 2; ++j) {
    coef(j, j - 1) = -2;
    coef(j, j) = -3;
    coef(j, j + 1) = 6;
    coef(j, j + 2) = -1;
  }

  coef(size - 2, size - 3) = -2;
  coef(size - 2, size - 2) = -3;
  coef(size - 2, size - 1) = 5;

  coef(size - 1, size - 2) = -2;
  coef(size - 1, size - 1) = 2;

  VectorXd fxb(size);
  for (int j = 0; j < size; ++j) {
    int jp1 = j + 1;
    const double Jacob =
        0.5 * (grid(column, jp1 + 1).y - grid(column, jp1 - 1).y);
    const double deltx = (grid(column, jp1).x - grid(column - 1, jp1).x);
    fxb(j) =
        6.0 * Jacob / deltx * (flow(column, jp1).u - flow(column - 1, jp1).u);
  }
  fxb *= -1.0;

  VectorXd new_v = coef.colPivHouseholderQr().solve(fxb);

  // update
  for (int j = 0; j < size; ++j)
    flow(column, j + 1).v = new_v(j);

  for (int j = 0; j < npoint_in_ydir; ++j)
    cout << flow(column, j).v << endl;
  cout << "end v" << endl;
  exit(EXIT_FAILURE);

  double res_v{0.0};
  for (int j = 1; j < npoint_in_ydir - 1; ++j)
    res_v += (flow(column, j).v - old_v[j]) * (flow(column, j).v - old_v[j]);
  return sqrt(res_v);
}

/**
double UpdateVthirdOrder2(VarArray<Variable, 2, 0> &flow, const int column,
                          const VarArray<Point, 2, 0> &grid) {

  int npoint_in_ydir{flow.Dim(1)};

  vector<double> old_v(npoint_in_ydir);
  for (int j = 1; j < npoint_in_ydir - 1; ++j)
    old_v[j] = flow(column, j).v;

  //   for (int j = 0; j < npoint_in_ydir; ++j)
  //     cout << flow(column, j).v << endl;
  //   cout << "end v" << endl;

  int size = npoint_in_ydir - 2;
  MatrixXd coef = MatrixXd::Zero(size, size);
  coef(0, 0) = -3;
  coef(0, 1) = 6;
  coef(0, 2) = -1;

  for (int j = 1; j < size - 2; ++j) {
    coef(j, j - 1) = -2;
    coef(j, j) = -3;
    coef(j, j + 1) = 6;
    coef(j, j + 2) = -1;
  }

  coef(size - 2, size - 3) = -2;
  coef(size - 2, size - 2) = -3;
  coef(size - 2, size - 1) = 6;

  coef(size - 1, size - 2) = -2;
  coef(size - 1, size - 1) = -3;

  VectorXd fxb(size);
  for (int j = 0; j < size-1; ++j) {
    int jp1 = j + 1;
    const double Jacob =
        0.5 * (grid(column, jp1 + 1).y - grid(column, jp1 - 1).y);
    const double deltx = (grid(column, jp1).x - grid(column - 1, jp1).x);
    fxb(j) =
        12.0 * Jacob / deltx * (flow(column, jp1).u - flow(column - 1, jp1).u) -
        2.0 * flow(column - 1, j).u - 3.0 * flow(column - 1, jp1).u +
        6.0 * flow(column - 1, jp1 + 1).u - flow(column - 1, jp1 + 2).u;
  }

  const double Jacob =
        0.5 * (grid(column, size + 1).y - grid(column, size - 1).y);
    const double deltx = (grid(column, size).x - grid(column - 1, size).x);
 fxb(size-1) =
        12.0 * Jacob / deltx * (flow(column, size).u - flow(column - 1, size).u)
- 2.0 * flow(column - 1, size-1).u - 3.0 * flow(column - 1, size).u + 6.0 *
flow(column - 1, size + 1).u - 1.0;


  fxb *= -1.0;

  VectorXd new_v = coef.colPivHouseholderQr().solve(fxb);

  // update
  for (int j = 0; j < size; ++j)
    flow(column, j + 1).v = new_v(j);

//   for (int j = 0; j < npoint_in_ydir; ++j)
//     cout << flow(column, j).v << endl;
//   cout << "end v" << endl;
//   exit(EXIT_FAILURE);

  double res_v{0.0};
  for (int j = 1; j < npoint_in_ydir - 1; ++j)
    res_v += (flow(column, j).v - old_v[j]) * (flow(column, j).v - old_v[j]);
  return sqrt(res_v);
}
**/
// 计算之前未知时间层需要使用已知时间层初始化
void CalcJacobianMatrix2(const VarArray<Variable, 2, 0> &flow, int column,
                         const VarArray<Point, 2, 0> &grid,
                         VarArray<double, 2, 0> &mat_coef,
                         const double re_num) {
  int npoint_in_ydir{grid.Dim(1)};
  const double C{1 / re_num};

  // 先统一处理 最后考虑首尾
  for (int j = 1; j < npoint_in_ydir - 1; ++j) {

    const double rJacob = 2.0 / (grid(column, j + 1).y - grid(column, j - 1).y);
    const double B =
        grid(column, j + 1).y - 2 * grid(column, j).y + grid(column, j - 1).y;
    const double rDeltx = 1.0 / (grid(column, j).x - grid(column - 1, j).x);
    // 第一次迭代第i层时间步上的值由上一层近视
    // if(iter_num != 1)

    mat_coef(0, j) = -0.5 * flow(column, j).v * rJacob - C * rJacob * rJacob -
                     0.5 * B * rJacob * rJacob * C * rJacob;
    mat_coef(0, j) *= 0.5;
    mat_coef(1, j) =
        (flow(column - 1, j).u) * rDeltx + 1.0 * C * rJacob * rJacob;
    mat_coef(2, j) = 0.5 * flow(column, j).v * rJacob - C * rJacob * rJacob +
                     0.5 * B * rJacob * rJacob * C * rJacob;
    mat_coef(2, j) *= 0.5;
  }
  //   cout<< flow(column, npoint_in_ydir-1).u << endl;
  // 首尾处理
  mat_coef(0, 1) = 0.0;
  mat_coef(2, npoint_in_ydir - 2) = 0.0;
}

// void CalcFXk2(const VarArray<Variable, 2, 0> &flow, int column,
//               const VarArray<Point, 2, 0> &grid, const double re_num,
//               vector<double> &fx_k)

// {
//   int npoint_in_ydir{grid.Dim(1)};
//   const double C{1 / re_num};

//   for (int j = 1; j < npoint_in_ydir - 1; ++j) {
//     const double rJacob = 2.0 / (grid(column, j + 1).y - grid(column, j -
//     1).y); const double B =
//         grid(column, j + 1).y - 2 * grid(column, j).y + grid(column, j -
//         1).y;
//     const double rDeltx = 1.0 / (grid(column, j).x - grid(column - 1, j).x);

//     const Variable &var_i_j{flow(column, j)}, var_im1_j{flow(column - 1, j)},
//         var_i_jm1{flow(column, j - 1)}, var_i_jp1{flow(column, j + 1)};

//     fx_k[j] = -
//   }
// }
